#include <iostream>
#include <string>
#include <fstream>
#include <iomanip>
#include <map>
#include <unistd.h>
#include <sys/stat.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <sophus/se3.hpp>
#include <pangolin/pangolin.h>

#include <trajectory.h>
#include <math_utils.h>

using namespace std;

//This constructor used for groundtruth
Trajectory::Trajectory(const string &fn):file_name(fn)
{
  if (read_from_file(fn)) {
    cout << file_name << " read failed! " << endl;
    //TODO
    return;
  }

  if (conf.IS_OUTPUT) {
    data_output();
    gnuplot_script_generate();
    matlab_script_generate();
  }
}

Trajectory::Trajectory(const string &fn, const Config &c):file_name(fn), conf(c)
{
  if (read_from_file(fn)) {
    cout << file_name << " read failed! " << endl;
    //TODO
    return;
  }

  //size() is a unsigned value
  //int t = conf.rs.size() - 1;
  if (id_rs < conf.rs.size()) {
    euler_reverse = conf.rs[id_rs++];
    cout << "euler reverse sign set to: " << euler_reverse << endl;
  }

  if (conf.IS_OUTPUT) {
    data_output();
    gnuplot_script_generate();
    matlab_script_generate();
  }
}

//TODO
void Trajectory::display_locs()
{
  if (!conf.IS_DISPLAY) {
    return;
  }

  if (stamp_poses.empty()) {
      cerr << "Trajectory is empty!" << endl;
      return;
  }

  map<int, pair<double, Sophus::SE3d>>::iterator it;
  for (it = stamp_poses.begin(); it != prev(stamp_poses.end()); it++) {
    //glColor3f(1, 1, 0);
    glColor3f(1 - (float)(it->first) / stamp_poses.size(), 0.0f,
                  (float)(it->first) / stamp_poses.size());
    glBegin(GL_LINES);
    auto p1 = it->second.second, p2 = next(it)->second.second;
    glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
    glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
    glEnd();
  }
}

string Trajectory::output_speci_name_get()
{
  string O_name;
  for (int i = 0; i < file_name.size(); i++) {
    if (isalpha(file_name[i]) || isdigit(file_name[i])) {
      O_name.append(1, file_name[i]);
    } else if (file_name[i]=='.' ||
      (i > 0 && file_name[i]=='/' && file_name[i-1]=='.')) {
      if (file_name.substr(i+1, file_name.length()).find('.') == string::npos) {
        break;
      }
      continue;
    } else {
      O_name.append(1, '_');
    }
  }
  return O_name;
}

int Trajectory::read_from_file(const string &file_name)
{
  Eigen::Quaterniond q; Eigen::Vector3d t;
  Sophus::SE3d T;
  string sTimestamp;
  double timestamp = 0;
  string::size_type sz;

  ifstream if_stream;
  if_stream.open(file_name.c_str());
  if (!if_stream.is_open()) {
    cout << "Read Trajectory file error: " << file_name <<endl;
    return -1;
  } else {
    int i = 0; 
    string sLineData;
    while ((getline(if_stream, sLineData)) && !sLineData.empty()) {
      istringstream strData(sLineData);
      strData >> sTimestamp >> t[0] >> t[1] >> t[2] >> q.x() >> q.y() >> q.z() >> q.w();
      if((sTimestamp == "#") || (sTimestamp == "-1.#INF")) {
          continue;
      }

      timestamp = stod(sTimestamp, &sz);

      T = Sophus::SE3d(q, t);
      stamp_poses[i++] = make_pair(timestamp,T);
    }
  }

  if_stream.close();
  return 0;
}

void Trajectory::data_output()
{
  //rotation order
  int ror1 = 2, ror2 = 0, ror3 = 1;
  switch (conf.rot_order) {
    case Config::ORDER_ZXY:
      break;
    case Config::ORDER_XYZ:
      ror1 = 0; ror2 = 1; ror3 = 2;
      break;
    default:
      cout << "ROT ORDER cannot be parsed! Set it to be ZXY" << endl;
  }

  string O_name = output_speci_name_get();

  if (access((conf.output_dir+"/data/").c_str(), 0)) {
    mkdir((conf.output_dir+"/data/").c_str(), 0775);
  }

  ofstream trajectory;
  string trajectory_s = conf.output_dir+"/data/"+O_name+"_trajectory.txt";
  trajectory.open(trajectory_s, ios::out|ios::trunc);
  if (!trajectory.is_open()) {
    cout << "failed open output file: " << trajectory_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "use -h options get more information!" << endl;
  } else {
    cout << "output data to file: " << trajectory_s << endl;
  }
  trajectory << "#timestamp tx ty tz rx ry rz dis" << endl;

  double dis = 0;

  Eigen::Vector3d trans_start(stamp_poses.begin()->second.second.translation());
  Eigen::Vector3d rot_start = stamp_poses.begin()->second.second.so3()
         .matrix().eulerAngles(ror1,ror2,ror3);

  Eigen::Matrix3d R_last = Eigen::Matrix3d::Identity();
  Eigen::Vector3d trans_last(0,0,0);
  Eigen::Vector3d rot_last(0,0,0);

  if (!conf.IS_INIT_ANGLE_0) {
    rot_last = rot_start;
    R_last = stamp_poses.begin()->second.second.so3().matrix();
  }
  trans_last = trans_start;

  //Eigen::Vector3d trans_last(stamp_poses[0].second.translation());
  //Eigen::Vector3d rot_last = stamp_poses[0].second.so3().matrix().eulerAngles(ror1,ror2,ror3);

  map<int, pair<double, Sophus::SE3d>>::iterator it;
  for (it = stamp_poses.begin(); it != stamp_poses.end(); it++) {
    //rx: re[1]; ry: re[2]; rz: re[0]
    Eigen::Matrix3d Re = it->second.second.so3().matrix();

    if (conf.IS_INIT_ANGLE_0) {
      Re = Re * stamp_poses.begin()->second.second.so3().matrix().transpose();
    }

    Eigen::Vector3d re = Re.eulerAngles(ror1,ror2,ror3);
    Eigen::Vector3d te = it->second.second.translation();

    double ts = it->second.first;

#if 0
    re = re - rot_start;
    te = te - trans_start;

    if (fabs(fabs(re[0]) - 2*M_PI) < 0.6) {
      if (re[0] > 0) {
        re[0] -= 2*M_PI;
      } else {
        re[0] += 2*M_PI;
      }
    }
    if (fabs(fabs(re[1]) - 2*M_PI) < 0.6) {
      if (re[1] > 0) {
        re[1] -= 2*M_PI;
      } else {
        re[1] += 2*M_PI;
      }
    }
    if (fabs(fabs(re[2]) - 2*M_PI) < 0.6) {
      if (re[2] > 0) {
        re[2] -= 2*M_PI;
      } else {
        re[2] += 2*M_PI;
      }
    }
#endif

    if (fabs(re[0] - rot_last[0]) > conf.e_r_sh
     || fabs(re[1] - rot_last[1]) > conf.e_r_sh
     || fabs(re[2] - rot_last[2]) > conf.e_r_sh) {

      if (fabs(fabs(fmod(re[0]-rot_last[0], 2*M_PI)) - M_PI) < conf.e_r_sh) {
        re[0] -= (int)((re[0]-rot_last[0]) / (2*M_PI)) * 2*M_PI;
      }
      if (fabs(fabs(fmod(re[1]+rot_last[1], 2*M_PI)) - M_PI) < conf.e_r_sh) {
        re[1] -= (int)((re[1]+rot_last[1]) / (2*M_PI)) * 2*M_PI;
      }
      if (fabs(fabs(fmod(re[2]-rot_last[2], 2*M_PI)) - M_PI) < conf.e_r_sh) {
        re[2] -= (int)((re[2]-rot_last[2]) / (2*M_PI)) * 2*M_PI;
      }

      if (fabs(fabs(re[0] - rot_last[0]) - M_PI) < conf.g_r_sh
       && fabs(fabs(re[1] + rot_last[1]) - M_PI) < conf.g_r_sh
       && fabs(fabs(re[2] - rot_last[2]) - M_PI) < conf.g_r_sh) {
        if (re[0] > rot_last[0]) {
          re[0] -= M_PI;
        } else {
          re[0] += M_PI;
        }

        if (re[1] + rot_last[1] > 0) {
          re[1] = M_PI - re[1];
        } else {
          re[1] = -M_PI - re[1];
        }

        if (re[2] > rot_last[2]) {
          re[2] -= M_PI;
        } else {
          re[2] += M_PI;
        }
      }

      if (fabs(fmod(fabs(re[1]), M_PI) - M_PI/2) < 1e-5) {
        re[0] = rot_last[0];
        re[2] = rot_last[2];
#if 0
      if (fmod(re[1], M_PI) > 0) {
        dd = (rot_last[0] - re[0] - (rot_last[2] - re[2])) / 2;
        re[0] += dd;
        re[2] -= dd;
      } else {
        dd = (rot_last[0] - re[0] + rot_last[2] - re[2]) / 2;
        re[0] += dd;
        re[2] += dd;
      }
#endif

      }

      if (IS_N2PI(re[0]-rot_last[0], conf.g_r_sh)) {
         re[0] -= round((re[0]-rot_last[0]) / (2*M_PI)) * 2*M_PI;
      }
      if (IS_N2PI(re[1]-rot_last[1], conf.g_r_sh)) {
         re[1] -= round((re[1]-rot_last[1]) / (2*M_PI)) * 2*M_PI;
      }
      if (IS_N2PI(re[2]-rot_last[2], conf.g_r_sh)) {
         re[2] -= round((re[2]-rot_last[2]) / (2*M_PI)) * 2*M_PI;
      }

      if (fabs(re[0] - rot_last[0]) > conf.g_r_sh
       || fabs(re[1] - rot_last[1]) > conf.g_r_sh
       || fabs(re[2] - rot_last[2]) > conf.g_r_sh) {
        cout << "meet a situation that can't be analyzed!" << endl;
        cout << "re: " << re[0] << " " << re[1] << " " << re[2] << endl;
        cout << "re_last: " << rot_last[0] << " " << rot_last[1] << " " << rot_last[2] << endl;
      }

    }

    dis +=
      sqrt(pow(te[0] - trans_last[0], 2)
         + pow(te[1] - trans_last[1], 2)
         + pow(te[2] - trans_last[2], 2));

    trajectory << setiosflags(ios::fixed) << setprecision(9) << ts << " "
               << te[0] << " " << te[1] << " " << te[2] << " "
               << re[0]/deg2rad << " " << re[1]/deg2rad << " " << re[2]/deg2rad << " " << dis << " " << endl;

    rot_last = re;
    trans_last = te;
  }

  trajectory.close();
}

void Trajectory::matlab_script_generate()
{
  string O_name = output_speci_name_get();

  if (access((conf.output_dir+"/matlab_scripts/").c_str(), 0)) {
    mkdir((conf.output_dir+"/matlab_scripts/").c_str(), 0775);
  }

  string matp_s = conf.output_dir+"/matlab_scripts/"+O_name+"_es_matlab.m";
  ofstream matp;
  matp.open(matp_s, ios::out|ios::trunc);
  if (!matp.is_open()) {
    cout << "Failed open output file: " << matp_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "Use -h options get more information!" << endl;
    return;
  } else {
    cout << "Generate matlab scripts: " << matp_s << endl;
  }

  string traj_fname = "..\\data\\" + O_name + "_trajectory.txt";

  matp << "clear;" << endl << endl
       << "traj_file = importdata('" << traj_fname << "');" << endl
       << "traj = traj_file.data;" << endl << endl

       << "figure(1);" << endl
       << "plot3(traj(:,2), traj(:,3), traj(:,4),'b','linewidth',1);" << endl
       << "axis equal;" << endl << "grid on;" << endl

       << "figure(2);" << endl
       << "subplot(1,2,1);" << endl
       << "plot(traj(:,3), traj(:,4), 'b', 'linewidth', 1);" << endl
       << "title(\"Top view of trajectories\")" << endl
       << "axis equal;" << endl << "grid on;" << endl
       << "subplot(1,2,2);" << endl
       << "plot(traj(:,2), traj(:,4), 'b', 'linewidth', 1);" << endl
       << "title(\"Side view of trajectories\")" << endl
       << "axis equal;" << endl << "grid on;" << endl
       << "legend('Estimate','Orientation','horizontal',"
          "'location',[0.13,0.01,0.36,0.05]);" << endl << endl

       << "figure(3);" << endl
       << "suptitle(\"Triaxial translocation\");" << endl
       << "subplot(3,1,1)" << endl
       << "plot(traj(:,8), traj(:,2),'b','linewidth',1);" << endl
       << "grid on;" << endl
       << "ylabel(\"X-axis\");" << endl
       << "subplot(3,1,2)" << endl
       << "plot(traj(:,8), traj(:,3),'b','linewidth',1);" << endl
       << "grid on;" << endl
       << "ylabel(\"Y-axis\");" << endl
       << "subplot(3,1,3)" << endl
       << "plot(traj(:,8), traj(:,4),'b','linewidth',1);" << endl
       << "grid on;" << endl
       << "ylabel(\"Z-axis\");" << endl
       << "xlabel(\"Distance to starting point, Unit(m)\");" << endl
       << "legend('Estimate','Orientation','horizontal',"
          "'location',[0.67,0.93,0.31,0.05]);" << endl << endl

       << "figure(4);" << endl
       << "suptitle(\"Euler angles\");" << endl
       << "subplot(3,1,1)" << endl
       << "plot(traj(:,8), traj(:,5),'b','linewidth',1);" << endl
       << "grid on;" << endl
       << "ylabel(\"Roll(\\circ)\");" << endl
       << "subplot(3,1,2)" << endl
       << "plot(traj(:,8), traj(:,6),'b','linewidth',1);" << endl
       << "grid on;" << endl
       << "ylabel(\"Pitch(\\circ)\");" << endl
       << "subplot(3,1,3)" << endl
       << "plot(traj(:,8), traj(:,7),'b','linewidth',1);" << endl
       << "grid on;" << endl
       << "ylabel(\"Yaw(\\circ)\");" << endl
       << "xlabel(\"Distance to starting point, Unit(m)\");" << endl
       << "legend('Estimate','Orientation','horizontal',"
          "'location',[0.67,0.93,0.31,0.05]);" << endl
       << endl;

  matp.close();
}

void Trajectory::gnuplot_script_generate()
{
  string O_name = output_speci_name_get();

  string gnup_s = conf.output_dir+"/"+O_name+"_gnuplot.sh";
  ofstream gnup;
  gnup.open(gnup_s, ios::out|ios::trunc);
  if (!gnup.is_open()) {
    cout << "failed open output file: " << gnup_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "use -h options get more information!" << endl;
    return;
  } else {
    cout << "generate gnuplot scripts: " << gnup_s << endl;
  }

  gnup << "#plot x y z trans" << endl
       << "set term wxt 2" << endl
       << "#set title \"" << O_name << " translations\"" << endl
       << "unset multiplot" << endl
       << "set font \"Times New Roman,10\"" << endl
       << "set multiplot" << endl
       << "set origin 0, 0.67" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_trajectory.txt\" "
          "u 8:2 lc 7 title 'tx' with line" << endl
       << "set grid" << endl
       << "set origin 0,0.33" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_trajectory.txt\" "
          "u 8:3 lc 2 title 'ty' with line" << endl
       << "set grid" << endl
       << "set origin 0,0" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_trajectory.txt\" "
          "u 8:4 lc 3 title 'tz' with line" << endl
       << "set grid" << endl
       << "unset multiplot" << endl
       << endl
       << "#plot x y z trans" << endl
       << "set term wxt 3" << endl
       << "#set title \"" << O_name << " euler\"" << endl
       << "unset multiplot" << endl
       << "set font \"Times New Roman,10\"" << endl
       << "set multiplot" << endl
       << "set origin 0, 0.67" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_trajectory.txt\" "
          "u 8:5 lc 7 title 'rx' with line" << endl
       << "set grid" << endl
       << "set origin 0,0.33" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_trajectory.txt\" "
          "u 8:6 lc 2 title 'ry' with line" << endl
       << "set grid" << endl
       << "set origin 0,0" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_trajectory.txt\" "
          "u 8:7 lc 3 title 'rz' with line" << endl
       << "set grid" << endl
       << "unset multiplot" << endl
       << endl
       << "#plot trajectory and euler" << endl
       << "reset session" << endl
       << "set term wxt 1" << endl
       << "set view equal xy" << endl
       << "set font \"Times New Roman,10\"" << endl
       << "splot \"data/" << O_name << "_trajectory.txt\" "
          "u 2:3:4 lc 3 title 'trajectory' with line" << endl;

  gnup.close();

  string bsh_s = conf.output_dir+"/"+O_name+"_plot.sh";
  ofstream bsh;
  bsh.open(bsh_s, ios::out|ios::trunc);
  if (!bsh.is_open()) {
    cout << "failed open output file: " << gnup_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "use -h options get more information!" << endl;
    return;
  } else {
    cout << "generate plot bash script: " << bsh_s << endl;
  }

  bsh << "#!/bin/bash" << endl
      << "gnuplot -e \"load '" << O_name << "_gnuplot.sh'\" --persist" << endl
      << "#gnuplot" << O_name << "_gnuplot.sh'\" --persist" << endl;

  bsh.close();
}

Es_traj::Es_traj(const string &fn, Gt_traj *gt, const Config &c):groundtruth(gt)
{
  file_name = fn;
  conf = c;

  //display color seed
  seed = random_seed++;

  if (read_from_file(fn)) {
    cout << file_name << " read failed! " << endl;
    //TODO
    return;
  }

  //int t = conf.rs.size() - 1;
  if (id_rs < conf.rs.size()) {
    euler_reverse = conf.rs[id_rs++];
    cout << "euler reverse sign set to: " << euler_reverse << endl;
  }

  if (gt && !gt->stamp_poses.empty()) {
    align_timestamp_to_gt();

    if (conf.IS_ALIGN) {

     // In gen script: Te * T = dT * Tg
     // So here gt for src, es for trg

      if (conf.IS_USE_FRANK) {
        //T{sSR, t}
        T_delta = FrankOpt(poses, poses_gt, R_, t_, s_, S_, conf.rot_weight,
                  conf.IS_ALIGN_SCALE, GAUSS_NEWTON);
        //T_delta = FrankOpt(poses, poses_gt, R_, t_, s_, S_, conf.IS_ALIGN_SCALE, DT_T_SIM);
        //T_delta = FrankOpt(poses, poses_gt, R_, t_, s_, S_, conf.IS_ALIGN_SCALE, T_FIRST);
        //T_delta = FrankOpt(poses, poses_gt, R_, t_, s_, S_, conf.IS_ALIGN_SCALE, DT_FIRST);
        //Re = S*dR*Rg*R^T*S => Rg = dR^T*S*Re*S*R
        //te = s*S*R*tg + t - Re^T*dt => tg = 1/s *R^T *S*(te - t + Re^T*dt)

        Eigen::Matrix3d SS = Eigen::Matrix3d::Identity();
        //when left-hand and right-hand frame occur
        //consisitent with U(x, 2) *= -1 that set the second row negitive
        if (S_ == -1) {
          cout << "S_ == -1!!!!!!!" << endl;
          SS(2, 2) = -1;
        }

        Eigen::Matrix3d dR = T_delta.so3().matrix();
        Eigen::Vector3d dt = T_delta.translation();

        cout << "Estimated results: " << endl;
        cout << "\tEstimated scale =\n"<< s_ << endl;
        cout << "\tEstimated rotation matrix =\n" << R_ << endl;
        cout << "\tEstimated translation =\n"<< t_ <<endl;
        cout << "\tEstimated dR =\n" << dR << endl;
        cout << "\tEstimated dt =\n"<< dt <<endl;

        map<int, Sophus::SE3d>::iterator it;
        for (it = poses.begin(); it != poses.end(); it++) {

          Eigen::Matrix3d R__ = dR.transpose() * SS * it->second.so3().matrix() * SS * R_;

          Eigen::Vector3d t_A = 1/s_ * R_.transpose() * SS *
            (it->second.translation() - t_ + it->second.so3().matrix().transpose() * dt);

          Sophus::SO3d R_A = Sophus::SO3d(R__);
          poses_align[it->first] = Sophus::SE3d(R_A, t_A);
        }

      } else if (conf.IS_USE_SALAS) {
        T_estimate = pose_estimation_3d3d_use_salas(poses, poses_gt, conf.rot_weight,
                     conf.IS_ROT_TRANSPOSE, conf.IS_ALIGN_SCALE);

        cout << "Estimated results: " << endl;
        cout << "\tEstimated scale =\n"<< T_estimate.scale() << endl;
        cout << "\tEstimated rotation matrix =\n" << T_estimate.rotationMatrix() << endl;
        cout << "\tEstimated translation =\n"<< T_estimate.translation() <<endl;

        map<int, Sophus::SE3d>::iterator it;
        for (it = poses.begin(); it != poses.end(); it++) {
          Sophus::Sim3d T1 = T_estimate.inverse();
          Eigen::Matrix3d R_A = Eigen::Matrix3d::Identity();
          if (conf.IS_ROT_TRANSPOSE) {
            //R_A = (T1.rotationMatrix() * it->second.so3().matrix().transpose()).transpose();
            R_A = it->second.so3().matrix() * T1.rotationMatrix().transpose();
          } else {
            R_A = T1.rotationMatrix() * it->second.so3().matrix();
          }

          Eigen::Vector3d t_A = T1.rxso3()*it->second.translation()
                              + T1.translation();

          poses_align[it->first] = Sophus::SE3d(R_A, t_A);
        }

      } else {

        //es = S*T * gt
        T_estimate = Umeym(poses, poses_gt, R_, t_, s_, S_,
                     conf.IS_USE_UMEYAMA, conf.IS_CONSIDER_ROT, conf.IS_ALIGN_SCALE);

        Eigen::Matrix3d SS = Eigen::Matrix3d::Identity();
        //TODO
          //when left-hand and right-hand frame occur
          //consisitent with U(x, 2) *= -1 that set the second row negitive
          if (S_ == -1) {
            cout << "S_ == -1!!!!!!!" << endl;
            SS(2, 2) = -1;
          }

        Eigen::Matrix3d Rg1 = poses_gt.begin()->second.so3().matrix();
        Eigen::Matrix3d Rc1 = poses.begin()->second.so3().matrix();

        Eigen::Matrix3d __R = Eigen::Matrix3d::Identity();
        Eigen::Matrix3d R__ = Eigen::Matrix3d::Identity();
        if (conf.IS_ROT_TRANSPOSE) {
          __R = Rg1 * R_.transpose() * SS * Rc1.transpose();
          R__ = SS * R_;
        } else {
          __R = R_.transpose() * SS;
          R__ = Rc1.transpose() * SS * R_ * Rg1;
        }

        map<int, Sophus::SE3d>::iterator it;
        for (it = poses.begin(); it != poses.end(); it++) {

          Eigen::Matrix3d R_AM = __R * it->second.so3().matrix() * R__;
          Eigen::Vector3d t_A = 1/s_ * R_.transpose() * SS * (it->second.translation() - t_);

          Sophus::SO3d R_A = Sophus::SO3d(R_AM);
          poses_align[it->first] = Sophus::SE3d(R_A, t_A);
        }
      }
    } else {
      //s_ = 1;
      //R_ = Eigen::Matrix3d::Identity();
      //T_estimate = Sophus::Sim3d(Sophus::RxSO3d(s_,R_), Eigen::Vector3d(0,0,0));
      map<int, Sophus::SE3d>::iterator it;
      for (it = poses.begin(); it != poses.end(); it++) {
        poses_align[it->first] = it->second;
      }
    }

    if (conf.IS_OUTPUT) {
      data_output_with_gt();
      gnuplot_script_generate_with_gt();
      matlab_script_generate_with_gt();
    }
  } else {
    //R_ = Sophus::SO3d(Eigen::Quaterniond(1,0,0,0));
    //R_ = Eigen::Matrix3d::Identity();
    R_ = stamp_poses.begin()->second.second.so3().matrix();
    t_ = stamp_poses.begin()->second.second.translation();
    //s_ = 1;
    T_estimate = Sophus::Sim3d(Sophus::RxSO3d(s_,R_), t_);
    for(int i = 0; i < stamp_poses.size(); i++) {
      poses[i] = stamp_poses[i].second;
      Sophus::SO3d R_A = Sophus::SO3d(R_.inverse() * poses[i].so3().matrix());
      Eigen::Vector3d t_A = T_estimate.inverse() * poses[i].translation();
      poses_align[i] = Sophus::SE3d(R_A, t_A);
    }

    if (conf.IS_OUTPUT) {
      data_output();
      gnuplot_script_generate();
      matlab_script_generate();
    }
  }
}

void Es_traj::display_locs()
{
  if (!conf.IS_DISPLAY) {
    return;
  }

  if (poses_align.empty()) {
    cerr << "Trajectory is empty!" << endl;
    return;
  }

  map<int, Sophus::SE3d>::iterator it;

  for (it = poses_align.begin(); it != prev(poses_align.end()); it++) {
    //glColor3f(1, 1, 0);
    glColor3f(1 - (float)(it->first) / poses_align.size(), 0.0f,
                  (float)(it->first) / poses_align.size());
    glBegin(GL_LINES);
    auto p1 = it->second, p2 = next(it)->second;
    glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
    glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
    glEnd();
  }

  uniform_real_distribution<double> cr(0, 1);
  default_random_engine cre(seed);

  double cx = cr(cre);
  double cy = cr(cre);
  double cz = cr(cre);

  if (conf.IS_PLOT_MATCHLINE && groundtruth && !groundtruth->stamp_poses.empty()) {
    for (it = poses_align.begin(); it != poses_align.end(); it++) {
      glColor3f(cx, cy, cz);
      glBegin(GL_LINES);
      auto p1 = it->second, p2 = groundtruth->stamp_poses[it->first].second;
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
  }
}

//slep or delete some poses to align Trajectory with timestamp
int Es_traj::align_timestamp_to_gt()
{
  double stD1, stD2;
  int i, j, k, index = 0;

  double es = stamp_poses.begin()->second.first;
  double ee = stamp_poses.rbegin()->second.first;
  double stDD1 = fabs(ee - es) / stamp_poses.size();
  double gs = groundtruth->stamp_poses.begin()->second.first;
  double ge = groundtruth->stamp_poses.rbegin()->second.first;
  double stDD2 = fabs(ge - gs) / groundtruth->stamp_poses.size();

  if (es > ge || gs > ee) {
    cout << "\n\033[1;33m" << "WARNING: timestamp cannot be aligned!\033[0m "
         << "\tTry calculation the offset automatically, If the results isn't what you want. "
         << "You should try using -t/--t-offset [timeoffset es-gt]!\n" << endl;

    double tt = stamp_poses.begin()->second.first
              - groundtruth->stamp_poses.begin()->second.first;
    double dtt = stDD2 > stDD1 ? stDD2 : stDD1;

    if (conf.t_offset == 0 && fabs(tt) > dtt) {
      conf.t_offset = tt;
    }
  }

  float c;
  Sophus::SE3d T;

  for(i = 0; i < stamp_poses.size() - 1; i++) {
    for(j = index; j < groundtruth->stamp_poses.size() - 2; j++) {
      stD1 = groundtruth->stamp_poses[j].first - stamp_poses[i].first + conf.t_offset;
      stD2 = groundtruth->stamp_poses[j+1].first - stamp_poses[i].first + conf.t_offset;

      //if oldest groundtruth already greater much than estimate, then break;
      if (stD1 >= stDD1 && stD1 >= stDD2) {
        break;
      }

      if (fabs(stD1) < fabs(stD2)) {
        index = j + 1;

        //stamp_poses[k].first; groundtruth->stamp_poses[j].first; stamp_poses[i].first;
        if (stD1 < 0) {
          k = i;
          do {
            k--;
            if (k < 0) {
              poses[j] = stamp_poses[0].second;
              //locations[j] = stamp_poses[0].second.translation();
              break;
            }
          } while (stamp_poses[k].first - conf.t_offset > groundtruth->stamp_poses[j].first);

          if (k >= 0) {
            c = (groundtruth->stamp_poses[j].first - stamp_poses[k].first + conf.t_offset)
                /(stamp_poses[i].first - stamp_poses[k].first);

            T = se_lerp(stamp_poses[k].second, stamp_poses[i].second, c);

            poses[j] = T;
            //locations[j] = T.translation();
          }
        } else if (stD1 > 0) {
          k = i;
          do {
            k++;
            if (k > stamp_poses.size() - 1) {
              //poses.push_back(stamp_poses[stamp_poses.size()-1].second);
              break;
            }
          } while (stamp_poses[k].first - conf.t_offset < groundtruth->stamp_poses[j].first);

          if (k > stamp_poses.size() - 1) {
            continue;
          }

          c = (groundtruth->stamp_poses[j].first - stamp_poses[i].first + conf.t_offset)
              /(stamp_poses[i].first - stamp_poses[k].first);

          T = se_lerp(stamp_poses[i].second, stamp_poses[k].second, c);
          poses[j] = T;
          //locations[j] = T.translation();

        //stamp_poses[i].first; groundtruth->stamp_poses[j].first; stamp_poses[k].first;
        } else {
          poses[j] = stamp_poses[i].second;
          //locations[j] = stamp_poses[0].second.translation();
        }

        poses_gt[j] = groundtruth->stamp_poses[j].second;
        timestamps_align[j] = groundtruth->stamp_poses[j].first;
        break;
      }
    }
  }
}

Sophus::Sim3d Es_traj::Umeym(map<int, Sophus::SE3d> &mpt,
                          map<int, Sophus::SE3d> &mps,
                          Eigen::Matrix3d& R_, Eigen::Vector3d& t_, double &s_, int &S_,
                          bool use_umeyama = false,
                          bool consider_rot = false,
                          bool align_scale = false)
{
  if (use_umeyama) {
    cout << "Align trajs using Umeyama." << endl;
    pose_estimation_3d3d_use_Umeyama(mpt, mps, R_, t_, s_, S_, align_scale);
  } else if (consider_rot && conf.rot_weight != 0) {
    cout << "The rotations will be consider to calculate Matrix." << endl;
    pose_estimation_3d3d_with_rot(mpt, mps, R_, t_, s_, S_, align_scale, conf.rot_weight,
                                  conf.IS_ROT_TRANSPOSE);
  } else {
    cout << "Align trajs with the staring point coincide." << endl;
    pose_estimation_3d3d(mpt, mps, R_, t_, s_, S_, align_scale);
  }

  cout << "Estimated results: " << endl;
  cout << "\tEstimated rotation matrix =\n" << R_ << endl;
  cout << "\tEstimated translation =\n"<< t_ <<endl;
  cout << "\tEstimated scale =\n"<< s_ << endl;

  //without SS 
  Sophus::Sim3d T(Sophus::RxSO3d(s_, R_), t_);
  //Sophus::Sim3d T((s_*R_).matrix(), t_);
  return T;
}

void Es_traj::data_output_with_gt()
{
  //rotation order
  int ror1 = 2, ror2 = 0, ror3 = 1;
  switch (conf.rot_order) {
    case Config::ORDER_ZXY:
      break;
    case Config::ORDER_XYZ:
      ror1 = 0; ror2 = 1; ror3 = 2;
      break;
    default:
      cout << "ROT ORDER cannot be parsed! Set it to be ZXY" << endl;
  }

  ofstream t_drift;

  //output file name;
  string O_name = output_speci_name_get();

  string t_drift_s = conf.output_dir+"/data/"+O_name+"_xyz_drift.txt";
  t_drift.open(t_drift_s, ios::out|ios::trunc);
  if (!t_drift.is_open()) {
    cout << "failed open output file: " << t_drift_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "use -h options get more information!" << endl;
  } else {
    cout << "output data to file: " << t_drift_s << endl;
  }

  t_drift << "#tx_drift ty_drift tz_drift error_drift"\
       " x_cumulative_drift y_cumulative_drift z_cumulative_drift"\
       " error_cumulative_drift distance timestamp" << endl;

  ofstream trajectories;
  string trajectories_s = conf.output_dir+"/data/"+O_name+"_trajectories.txt";
  trajectories.open(trajectories_s, ios::out|ios::trunc);
  if (!trajectories.is_open()) {
    cout << "failed open output file: " << trajectories_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "use -h options get more information!" << endl;
  } else {
    cout << "output data to file: " << trajectories_s << endl;
  }
  trajectories << "#groundtruth_align estimate_Trajectory_align distance timestamp" << endl;

  ofstream euler;
  string euler_s = conf.output_dir+"/data/"+O_name+"_euler.txt";
  euler.open(euler_s, ios::out|ios::trunc);
  if (!euler.is_open()) {
    cout << "failed open output file: " << euler_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "use -h options get more information!" << endl;
  } else {
    cout << "output data to file: " << euler_s << endl;
  }
  euler << "#euler rx_truth ry_truth rz_truth rx_estimate ry_estimate rz_estimate"\
           " rx_error ry_error rz_error angle_groundtruth angle_estimate distance timestamp" << endl;

  ofstream evaluation_results;
  string evaluation_results_s = conf.output_dir+"/data/"+O_name+"_evaluation_results.txt";
  evaluation_results.open(evaluation_results_s, ios::out|ios::trunc);
  if (!evaluation_results.is_open()) {
    cout << "failed open output file: " << evaluation_results_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "use -h options get more information!" << endl;
  } else {
    cout << "output data to file: " << evaluation_results_s << endl;
  }
  //min is 0 at the start point
  evaluation_results << "#evaluation_results xyzS(mean max) rpyA(mean max) Trajectory rmse" << endl;

  double tx_drift_rec = 0, ty_drift_rec = 0, tz_drift_rec = 0;
  double rx_error_rec = 0, ry_error_rec = 0, rz_error_rec = 0;
  double tx_mean = 0, ty_mean = 0, tz_mean = 0;
  double tx_max = 0, ty_max = 0, tz_max = 0;
  double rx_mean_ape = 0, ry_mean_ape = 0, rz_mean_ape = 0;
  double rx_mean_rte = 0, ry_mean_rte = 0, rz_mean_rte = 0;
  double rx_max_cum = 0, ry_max_cum = 0, rz_max_cum = 0;
  double traj_ape_mean = 0, ape_max = 0;
  double traj_rte_mean = 0, rte_max = 0;
  double angle_mean = 0, angle_max = 0, angle_mean_ape = 0, angle_mean_rte = 0;
  double dis = 0;

  Eigen::Matrix3d Re_start = poses_align.begin()->second.so3().matrix();
  Eigen::Vector3d re_start = Re_start.eulerAngles(ror1,ror2,ror3);
  Eigen::Matrix3d Rg_start = Eigen::Matrix3d::Identity();
  Eigen::Vector3d rg_start(0,0,0);

  double w = poses_align.begin()->second.so3().unit_quaternion().w();
  double angle_est_start = 2*acos(min(max(w, -1.0), 1.0));
  w = poses_gt.begin()->second.so3().unit_quaternion().w();
  double angle_grt_start = 2*acos(min(max(w, -1.0), 1.0));
  //double angle_est_start = 2*acos(poses_align.begin()->second.so3().unit_quaternion().w());
  //double angle_grt_start = 2*acos(poses_gt.begin()->second.so3().unit_quaternion().w());

  double angle_est_last = 0, angle_grt_last = 0;

  if (poses_gt.size() > 1) {
    Rg_start = poses_gt.begin()->second.so3().matrix();
    rg_start = Rg_start.eulerAngles(ror1,ror2,ror3);
  } else {
    Rg_start = Re_start;
    rg_start = Re_start.eulerAngles(ror1,ror2,ror3);
  }

  double x_last = poses_gt.begin()->second.translation()[0];
  double y_last = poses_gt.begin()->second.translation()[1];
  double z_last = poses_gt.begin()->second.translation()[2];

  double rgx_last = 0, rgy_last = 0, rgz_last = 0;
  double rex_last = 0, rey_last = 0, rez_last = 0;

  if (!conf.IS_INIT_ANGLE_0) {
    rgx_last = rg_start[0]; rgy_last = rg_start[1]; rgz_last = rg_start[2];
    rex_last = re_start[0]; rey_last = re_start[1]; rez_last = re_start[2];
  }

  //here align gt to estimate
  map<int, Sophus::SE3d>::iterator it;
  for (it = poses_align.begin(); it != poses_align.end(); it++) {
    Sophus::SE3d gt_i;

    if (poses_gt.size() > 1) {
      gt_i = poses_gt[it->first];
    } else {
      gt_i = it->second;
    }

    //Sophus::SE3d gt2es_i = Sophus::SE3d(T_estimate.matrix() * gt_i.matrix());
    //Sophus::SO3d R_A = Sophus::SO3d(R_ * gt_i.so3().matrix());
    //Eigen::Vector3d tA = T_estimate * gt_i.translation();
    //Sophus::SE3d gt2es_i = Sophus::SE3d(R_A, tA);

    Eigen::Matrix3d Rg = gt_i.so3().matrix();
    Eigen::Matrix3d Re = it->second.so3().matrix();

    if (conf.IS_INIT_ANGLE_0) {
      Rg = Rg * Rg_start.transpose();
      Re = Re * Re_start.transpose();
    }

    Eigen::Vector3d rg = Rg.eulerAngles(ror1,ror2,ror3);
    Eigen::Vector3d re = Re.eulerAngles(ror1,ror2,ror3);

    double angle_est = 2*acos(min(max(it->second.so3().unit_quaternion().w(), -1.0), 1.0));
    double angle_grt = 2*acos(min(max(gt_i.so3().unit_quaternion().w(), -1.0), 1.0));

    angle_grt -= angle_grt_start;
    angle_est -= angle_est_start;

    if (angle_grt < 0) {
      angle_grt = -angle_grt;
    }
    if (angle_grt > M_PI && angle_grt < 2*M_PI) {
       angle_grt = 2*M_PI - angle_grt;
    }
    if (angle_grt > 2*M_PI) {
      angle_grt = fmod(angle_grt, 2*M_PI);
    }

    if (angle_est < 0) {
      angle_est = -angle_est;
    }
    if (angle_est > M_PI && angle_est < 2*M_PI) {
       angle_est = 2*M_PI - angle_est;
    }
    if (angle_est > 2*M_PI) {
      angle_est = fmod(angle_est, 2*M_PI);
    }

    double rgx = rg[0];
    double rgy = rg[1];
    double rgz = rg[2];
    double rex = re[0];
    double rey = re[1];
    double rez = re[2];

    if (fabs(rgx - rgx_last) > conf.g_r_sh
     || fabs(rgy - rgy_last) > conf.g_r_sh
     || fabs(rgz - rgz_last) > conf.g_r_sh) {

      if (fabs(fabs(fmod(rgx-rgx_last, 2*M_PI)) - M_PI) < conf.g_r_sh) {
        rgx -= (int)((rgx-rgx_last) / (2*M_PI)) * 2*M_PI;
      }
      if (fabs(fabs(fmod(rgy+rgy_last, 2*M_PI)) - M_PI) < conf.g_r_sh) {
        rgy -= (int)((rgy+rgy_last) / (2*M_PI)) * 2*M_PI;
      }
      if (fabs(fabs(fmod(rgz-rgz_last, 2*M_PI)) - M_PI) < conf.g_r_sh) {
        rgz -= (int)((rgz-rgz_last) / (2*M_PI)) * 2*M_PI;
      }

      if (fabs(fabs(rgx - rgx_last) - M_PI) < conf.g_r_sh
       && fabs(fabs(rgy + rgy_last) - M_PI) < conf.g_r_sh
       && fabs(fabs(rgz - rgz_last) - M_PI) < conf.g_r_sh) {
        if (rgx > rgx_last) {
          rgx -= M_PI;
        } else {
          rgx += M_PI;
        }

        if (rgy + rgy_last > 0) {
          rgy = M_PI - rgy;
        } else {
          rgy = -M_PI - rgy;
        }

        if (rgz > rgz_last) {
          rgz -= M_PI;
        } else {
          rgz += M_PI;
        }
      }

      if (fabs(fmod(fabs(rgy), M_PI) - M_PI/2) < 1e-5) {
         rgx = rgx_last;
         rgz = rgz_last;
      }

      if (IS_N2PI(rgx-rgx_last, conf.g_r_sh)) {
         rgx -= round((rgx-rgx_last) / (2*M_PI)) * 2*M_PI;
      }
      if (IS_N2PI(rgy-rgy_last, conf.g_r_sh)) {
         rgy -= round((rgy-rgy_last) / (2*M_PI)) * 2*M_PI;
      }
      if (IS_N2PI(rgz-rgz_last, conf.g_r_sh)) {
         rgz -= round((rgz-rgz_last) / (2*M_PI)) * 2*M_PI;
      }

      if (fabs(rgx - rgx_last) > conf.g_r_sh
       || fabs(rgy - rgy_last) > conf.g_r_sh
       || fabs(rgz - rgz_last) > conf.g_r_sh) {
        cout << "meet a situation that can't be analyzed!" << endl;
        cout << "rg: " << rgx << " " << rgy << " " << rgz << endl;
        cout << "rg_last: " << rgx_last << " " << rgy_last << " " << rgz_last << endl;
      }
    }

    if (fabs(rex - rex_last) > conf.e_r_sh
     || fabs(rey - rey_last) > conf.e_r_sh
     || fabs(rez - rez_last) > conf.e_r_sh) {

      if (fabs(fabs(fmod(rex-rex_last, 2*M_PI)) - M_PI) < conf.e_r_sh) {
        rex -= (int)((rex-rex_last) / (2*M_PI)) * 2*M_PI;
      }
      if (fabs(fabs(fmod(rey+rey_last, 2*M_PI)) - M_PI) < conf.e_r_sh) {
        rey -= (int)((rey+rey_last) / (2*M_PI)) * 2*M_PI;
      }
      if (fabs(fabs(fmod(rez-rez_last, 2*M_PI)) - M_PI) < conf.e_r_sh) {
        rez -= (int)((rez-rez_last) / (2*M_PI)) * 2*M_PI;
      }

      if (fabs(fabs(rex - rex_last) - M_PI) < conf.e_r_sh
       && fabs(fabs(rey + rey_last) - M_PI) < conf.e_r_sh
       && fabs(fabs(rez - rez_last) - M_PI) < conf.e_r_sh) {
        if (rex > rex_last) {
          rex -= M_PI;
        } else {
          rex += M_PI;
        }

        if (rey + rey_last > 0) {
          rey = M_PI - rey;
        } else {
          rey = -M_PI - rey;
        }

        if (rez > rez_last) {
          rez -= M_PI;
        } else {
          rez += M_PI;
        }
      }

      if (fabs(fmod(fabs(rey), M_PI) - M_PI/2) < 1e-5) {
        rex = rex_last;
        rez = rez_last;
      }

      if (IS_N2PI(rex-rex_last, conf.e_r_sh)) {
         rex -= round((rex-rex_last) / (2*M_PI)) * 2*M_PI;
      }
      if (IS_N2PI(rey-rey_last, conf.e_r_sh)) {
         rey -= round((rey-rey_last) / (2*M_PI)) * 2*M_PI;
      }
      if (IS_N2PI(rez-rez_last, conf.e_r_sh)) {
         rez -= round((rez-rez_last) / (2*M_PI)) * 2*M_PI;
      }

      if (fabs(rex - rex_last) > conf.e_r_sh
       || fabs(rey - rey_last) > conf.e_r_sh
       || fabs(rez - rez_last) > conf.e_r_sh) {
        cout << "meet a situation that can't be analyzed!" << endl;
        cout << "re: " << rex << " " << rey << " " << rez << endl;
        cout << "re_last: " << rex_last << " " << rey_last << " " << rez_last << endl;
      }

    }

#if 0
    if (fabs(fabs(angle_est - angle_grt) - 2*M_PI) < 0.6) {
      if (angle_est - angle_grt > 0) {
        angle_est -= 2*M_PI;
      } else {
        angle_est += 2*M_PI;
      }
    }

    if (fabs(fabs(rex + rgx) - M_PI) < 0.6
     && fabs(fabs(rey - rgy) - M_PI) < 0.6
     && fabs(fabs(rez - rgz) - M_PI) < 0.6) {
      if (rex + rgx > 0) {
        rex = M_PI - rex;
      } else {
        rex = -M_PI - rex;
      }

      if (rey > rgy) {
        rey -= M_PI;
      } else {
        rey += M_PI;
      }

      if (rez > rgz) {
        rez -= M_PI;
      } else {
        rez += M_PI;
      }
    }

    if (fabs(fabs(rex + rex_last) - M_PI) < 0.6
     && fabs(fabs(rey - rey_last) - M_PI) < 0.6
     && fabs(fabs(rez - rez_last) - M_PI) < 0.6) {
      if (rex + rgx_last > 0) {
        rex = M_PI - rex;
      } else {
        rex = -M_PI - rex;
      }

      if (rey > rgy_last) {
        rey -= M_PI;
      } else {
        rey += M_PI;
      }

      if (rez > rgz_last) {
        rez -= M_PI;
      } else {
        rez += M_PI;
      }
    }

    if (fabs(fabs(rex - rgx) - 2*M_PI) < 0.8) {
      if (rex - rgx > 0) {
        rex -= 2*M_PI;
      } else {
        rex += 2*M_PI;
      }
    }
    if (fabs(fabs(rey - rgy) - 2*M_PI) < 0.8) {
      if (rey - rgy > 0) {
        rey -= 2*M_PI;
      } else {
        rey += 2*M_PI;
      }
    }
    if (fabs(fabs(rez - rgz) - 2*M_PI) < 0.8) {
      if (rez - rgz > 0) {
        rez -= 2*M_PI;
      } else {
        rez += 2*M_PI;
      }
    }

    if (rgx < -M_PI - 0.6) {
      rgx += 2*M_PI;
    } else if (rgx > M_PI + 0.6) {
      rgx -= 2*M_PI;
    }
    if (rgy < -M_PI - 0.6) {
      rgy += 2*M_PI;
    } else if (rgy > M_PI + 0.6) {
      rgy -= 2*M_PI;
    }
    if (rgz < -M_PI - 0.6) {
      rgz += 2*M_PI;
    } else if (rgz > M_PI + 0.6) {
      rgz -= 2*M_PI;
    }

    if (rex < -M_PI - 0.6) {
      rex += 2*M_PI;
    } else if (rex > M_PI + 0.6) {
      rex -= 2*M_PI;
    }
    if (rey < -M_PI - 0.6) {
      rey += 2*M_PI;
    } else if (rey > M_PI + 0.6) {
      rey -= 2*M_PI;
    }
    if (rez < -M_PI - 0.6) {
      rez += 2*M_PI;
    } else if (rez > M_PI + 0.6) {
      rez -= 2*M_PI;
    }
#endif

    dis +=
      sqrt(pow(gt_i.translation()[0] - x_last, 2)
         + pow(gt_i.translation()[1] - y_last, 2)
         + pow(gt_i.translation()[2] - z_last, 2));

    double edis_cum =
      sqrt(pow(gt_i.translation()[0] - it->second.translation()[0], 2)
         + pow(gt_i.translation()[1] - it->second.translation()[1], 2)
         + pow(gt_i.translation()[2] - it->second.translation()[2], 2));

    double edis =
    sqrt(pow(gt_i.translation()[0] - it->second.translation()[0] - tx_drift_rec, 2)
     + pow(gt_i.translation()[1] - it->second.translation()[1] - ty_drift_rec, 2)
     + pow(gt_i.translation()[2] - it->second.translation()[2] - tz_drift_rec, 2));

    double ts = timestamps_align[it->first];

    trajectories << gt_i.translation()[0] << " "
                 << gt_i.translation()[1] << " "
                 << gt_i.translation()[2] << " "
                 << it->second.translation()[0] << " "
                 << it->second.translation()[1] << " "
                 << it->second.translation()[2] << " " << dis << " "
                 << setiosflags(ios::fixed) << setprecision(9) << ts << endl;

    double tx_drift = gt_i.translation()[0] - it->second.translation()[0];
    double ty_drift = gt_i.translation()[1] - it->second.translation()[1];
    double tz_drift = gt_i.translation()[2] - it->second.translation()[2];

    t_drift
      << tx_drift - tx_drift_rec << " "
      << ty_drift - ty_drift_rec << " "
      << tz_drift - tz_drift_rec << " "
      << edis << " "
      << tx_drift << " "
      << ty_drift << " "
      << tz_drift << " "
      << edis_cum << " " << dis << " "
	  << setiosflags(ios::fixed) << setprecision(9) << ts << endl;

    tx_max = (fabs(tx_drift) > tx_max) ? fabs(tx_drift) : tx_max;
    tx_mean += fabs(tx_drift);
    ty_max = (fabs(ty_drift) > ty_max) ? fabs(ty_drift) : ty_max;
    ty_mean += fabs(ty_drift);
    tz_max = (fabs(tz_drift) > tz_max) ? fabs(tz_drift) : tz_max;
    tz_mean += fabs(tz_drift);
    ape_max = (edis_cum > ape_max) ? edis_cum : ape_max;
    traj_ape_mean += edis_cum;
    rte_max = (edis > rte_max) ? edis : rte_max;
    traj_rte_mean += edis;

    rgx_last = rgx;
    rgy_last = rgy;
    rgz_last = rgz;
    rex_last = rex;
    rey_last = rey;
    rez_last = rez;

    double rx_error_cum = rex - rgx;
    double ry_error_cum = rey - rgy;
    double rz_error_cum = rez - rgz;

    if (fabs(rx_error_cum) > 2*M_PI) {
      rx_error_cum = fmod(rx_error_cum, 2*M_PI);
    }
    if (fabs(ry_error_cum) > 2*M_PI) {
      ry_error_cum = fmod(ry_error_cum, 2*M_PI);
    }
    if (fabs(rz_error_cum) > 2*M_PI) {
      rz_error_cum = fmod(rz_error_cum, 2*M_PI);
    }

    double rx_error = rx_error_cum - rx_error_rec;
    double ry_error = ry_error_cum - ry_error_rec;
    double rz_error = rz_error_cum - rz_error_rec;

    euler
      << rgx/deg2rad << " " << rgy/deg2rad << " " << rgz/deg2rad << " "
      << rex/deg2rad << " " << rey/deg2rad << " " << rez/deg2rad << " "
      << rx_error_cum/deg2rad << " " << ry_error_cum/deg2rad << " "
                                     << rz_error_cum/deg2rad << " "
      << angle_grt/deg2rad << " " << angle_est/deg2rad
      << " " << dis << " "
	  << setiosflags(ios::fixed) << setprecision(9) << ts << endl;

    rx_max_cum = (fabs(rx_error_cum) > rx_max_cum) ? fabs(rx_error_cum) : rx_max_cum;
    rx_mean_ape += fabs(rx_error_cum);
    rx_mean_rte += fabs(rx_error);
    ry_max_cum = (fabs(ry_error_cum) > ry_max_cum) ? fabs(ry_error_cum) : ry_max_cum;
    ry_mean_ape += fabs(ry_error_cum);
    ry_mean_rte += fabs(ry_error);
    rz_max_cum = (fabs(rz_error_cum) > rz_max_cum) ? fabs(rz_error_cum) : rz_max_cum;
    rz_mean_ape += fabs(rz_error_cum);
    rz_mean_rte += fabs(rz_error);


    double angle_error = fabs(angle_grt - angle_est);
    angle_max = (fabs(angle_error) > angle_max) ? fabs(angle_error) : angle_max;
    angle_mean += angle_error;
    angle_mean_ape += sqrt(pow(rx_error_cum, 2) + pow(ry_error_cum, 2) + pow(rz_error_cum, 2));
    angle_mean_rte += sqrt(pow(rx_error, 2) + pow(ry_error, 2) + pow(rz_error, 2));

    tx_drift_rec = tx_drift;
    ty_drift_rec = ty_drift;
    tz_drift_rec = tz_drift;
    rx_error_rec = rx_error_cum;
    ry_error_rec = ry_error_cum;
    rz_error_rec = rz_error_cum;

    x_last = gt_i.translation()[0];
    y_last = gt_i.translation()[1];
    z_last = gt_i.translation()[2];
  }

  tx_mean /= poses.size();
  ty_mean /= poses.size();
  tz_mean /= poses.size();
  rx_mean_rte /= poses.size();
  ry_mean_rte /= poses.size();
  rz_mean_rte /= poses.size();
  rx_mean_ape /= poses.size();
  ry_mean_ape /= poses.size();
  rz_mean_ape /= poses.size();
  angle_mean_ape /= poses.size();
  angle_mean_rte /= poses.size();

  traj_ape_mean /= poses.size();
  traj_rte_mean /= poses.size();
  angle_mean /= poses.size();

  evaluation_results
    << "Trajs error (Unit: m); Angles error (Unit: deg)" << endl
    << "tx_mean: " << tx_mean << endl
    << "tx_max: " << tx_max << endl
    << "ty_mean: " << ty_mean << endl
    << "ty_max: " << ty_max << endl
    << "tz_mean: " << tz_mean << endl
    << "tz_max: " << tz_max << endl
    << "trajecroty rmse(APE): " << traj_ape_mean << endl
    << "trajecroty rmse(RTE): " << traj_rte_mean << endl
    << "traj_error_max(APE): " << ape_max << endl
    << "traj_drift_max(RTE): " << rte_max << endl
    << endl
    << "rx_mean_ape: " << rx_mean_ape/deg2rad << endl
    << "rx_mean_rte: " << rx_mean_rte/deg2rad << endl
    << "rx_max_cum: " << rx_max_cum/deg2rad << endl
    << "ry_mean_ape: " << ry_mean_ape/deg2rad << endl
    << "ry_mean_rte: " << ry_mean_rte/deg2rad << endl
    << "ry_max_cum: " << ry_max_cum/deg2rad << endl
    << "rz_mean_ape: " << rz_mean_ape/deg2rad << endl
    << "rz_mean_rte: " << rz_mean_rte/deg2rad << endl
    << "rz_max_cum: " << rz_max_cum/deg2rad << endl
    << "angle_rmse(axis angle): " << angle_mean/deg2rad << endl
    << "angle_rmse(Euler angles APE): " << angle_mean_ape/deg2rad << endl
    << "angle_rmse(Euler angles RTE): " << angle_mean_rte/deg2rad << endl
    << "angle_max: " << angle_max/deg2rad << endl;

  cout << "traj rmse: \n" << traj_ape_mean << endl;
  cout << "angle rmse(aixs angle (rad)): \n" << angle_mean << endl;
  cout << "angle rmse(Euler angles (rad)): \n" << angle_mean_ape << endl;

  t_drift.close();
  trajectories.close();
  euler.close();
  evaluation_results.close();
}

void Es_traj::matlab_script_generate_with_gt()
{
  //output file name;
  string O_name = output_speci_name_get();

  if (access((conf.output_dir+"/matlab_scripts/").c_str(), 0)) {
    mkdir((conf.output_dir+"/matlab_scripts/").c_str(), 0775);
  }

  string matp_s = conf.output_dir+"/matlab_scripts/"+O_name+"_es2gt_matlab.m";
  ofstream matp;
  matp.open(matp_s, ios::out|ios::trunc);
  if (!matp.is_open()) {
    cout << "Failed open output file: " << matp_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "Use -h options get more information!" << endl;
    return;
  } else {
    cout << "Generate matlab scripts: " << matp_s << endl;
  }

  string traj_fname = "..\\data\\" + O_name + "_trajectories.txt";
  string t_drift_fname = "..\\data\\"+O_name + "_xyz_drift.txt";
  string angles_fname = "..\\data\\"+O_name + "_euler.txt";

  matp << "clear;" << endl << endl
       << "traj_file = importdata('" << traj_fname << "');" << endl
       << "traj = traj_file.data;" << endl << endl

       << "figure(5);" << endl
       << "subplot(1,2,1);" << endl
       << "plot(traj(:,1), traj(:,3), 'k', 'linewidth', 1);" << endl
       << "hold on;" << endl
       << "plot(traj(:,4), traj(:,6), 'b', 'linewidth', 1);" << endl;

  if (!conf.IS_USE_UMEYAMA) {
    matp << "hold on;" << endl
         << "plot(traj(1,1), traj(1,3),'^k','MarkerFaceColor','k');" << endl
         << "hold off;" << endl;
  }

  matp << "title(\"Top view of trajectories\")" << endl
       << "axis equal;" << endl << "grid on; hold off;" << endl
       << "subplot(1,2,2);" << endl
       << "plot(traj(:,2), traj(:,3), 'k', 'linewidth', 1);" << endl
       << "hold on;" << endl
       << "plot(traj(:,5), traj(:,6), 'b', 'linewidth', 1);" << endl;

  if (!conf.IS_USE_UMEYAMA) {
    matp << "hold on;" << endl
         << "plot(traj(1,2), traj(1,3),'^k','MarkerFaceColor','k');" << endl
         << "hold off;" << endl;
  }

  matp << "title(\"Side view of trajectories\")" << endl
       << "axis equal;" << endl << "grid on; hold off;" << endl;

  if (!conf.IS_USE_UMEYAMA) {
    matp << "legend('Groundtruth','Estimate','Srating point','Orientation','horizontal',";
  } else {
    matp << "legend('Groundtruth','Estimate','Orientation','horizontal',";
  }

  matp <<  "'location',[0.13,0.01,0.36,0.05]);" << endl << endl

       << "figure(6);" << endl
       << "suptitle(\"Triaxial translocation\");" << endl
       << "subplot(3,1,1)" << endl
       << "plot(traj(:,7), traj(:,1),'k','linewidth',1);" << endl
       << "hold on;" << endl
       << "plot(traj(:,7), traj(:,4),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"X-axis\");" << endl
       << "subplot(3,1,2)" << endl
       << "plot(traj(:,7), traj(:,2),'k','linewidth',1);" << endl
       << "hold on;" << endl
       << "plot(traj(:,7), traj(:,5),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"Y-axis\");" << endl
       << "subplot(3,1,3)" << endl
       << "plot(traj(:,7), traj(:,3),'k','linewidth',1);" << endl
       << "hold on;" << endl
       << "plot(traj(:,7), traj(:,6),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"Z-axis\");" << endl
       << "xlabel(\"Distance to starting point, Unit(m)\");" << endl
       << "legend('Groundtruth','Estimate','Orientation','horizontal',"
          "'location',[0.67,0.93,0.31,0.05]);" << endl << endl

       << "drift_file = importdata('" << t_drift_fname << "');" << endl
       << "drift = drift_file.data;" << endl << endl

       << "figure(7);" << endl
       << "subplot(2,1,1);" << endl
       << "plot(drift(:,9), drift(:,1),'r','linewidth',1);" << endl
       << "grid on;hold on;" << endl
       << "plot(drift(:,9), drift(:,2),'g','linewidth',1);" << endl
       << "grid on;hold on;" << endl
       << "plot(drift(:,9), drift(:,3),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"Triaxial drift, Unit(m)\");" << endl

       << "subplot(2,1,2);" << endl
       << "plot(drift(:,9), drift(:,5),'r','linewidth',1);" << endl
       << "grid on;hold on;" << endl
       << "plot(drift(:,9), drift(:,6),'g','linewidth',1);" << endl
       << "grid on;hold on;" << endl
       << "plot(drift(:,9), drift(:,7),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"Triaxial cumulative drift, Unit(m)\");" << endl
       << "xlabel(\"Distance to starting point, Unit(m)\");" << endl
       << "legend('X-axis','Y-axis','Z-axis','Orientation','horizontal',"
          "'location',[0.2,0.95,0.3,0.05]);" << endl << endl

       << "angles_file = importdata('" << angles_fname << "');" << endl
       << "angles = angles_file.data;" << endl << endl

       << "figure(8);" << endl
       << "suptitle(\"Euler angles\");" << endl
       << "subplot(3,1,1)" << endl
       << "plot(angles(:,12), angles(:,1),'k','linewidth',1);" << endl
       << "hold on;" << endl
       << "plot(angles(:,12), angles(:,4),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"Roll(\\circ)\");" << endl
       << "subplot(3,1,2)" << endl
       << "plot(angles(:,12), angles(:,2),'k','linewidth',1);" << endl
       << "hold on;" << endl
       << "plot(angles(:,12), angles(:,5),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"Pitch(\\circ)\");" << endl
       << "subplot(3,1,3)" << endl
       << "plot(angles(:,12), angles(:,3),'k','linewidth',1);" << endl
       << "hold on;" << endl
       << "plot(angles(:,12), angles(:,6),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"Yaw(\\circ)\");" << endl
       << "xlabel(\"Distance to starting point, Unit(m)\");" << endl
       << "legend('Groundtruth','Estimate','Orientation','horizontal',"
          "'location',[0.67,0.93,0.31,0.05]);" << endl << endl

       << "figure(9);" << endl
       << "subplot(2,1,1);" << endl
       << "plot(angles(:,12), angles(:,7),'r','linewidth',1);" << endl
       << "grid on;hold on;" << endl
       << "plot(angles(:,12), angles(:,8),'g','linewidth',1);" << endl
       << "grid on;hold on;" << endl
       << "plot(angles(:,12), angles(:,9),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "title(\"Euler angles error\");" << endl
       << "legend('X-axis','Y-axis','Z-axis','Orientation','horizontal',"
          "'location','best');" << endl

       << "subplot(2,1,2);" << endl
       << "plot(angles(:,12), angles(:,10),'k','linewidth',1);" << endl
       << "grid on; hold on;" << endl
       << "plot(angles(:,12), angles(:,11),'b','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "title(\"Axial angle\");" << endl
       << "xlabel(\"Distance to starting point, Unit(m)\");" << endl
       << "legend('Groundtruth','Estimate','Orientation','horizontal',"
          "'location','best');" << endl;
  matp.close();
}

void Es_traj::gnuplot_script_generate_with_gt()
{
  //output file name;
  string O_name = output_speci_name_get();

  string gnup_s = conf.output_dir+"/"+O_name+"_es2gt_gnuplot.sh";
  ofstream gnup;
  gnup.open(gnup_s, ios::out|ios::trunc);
  if (!gnup.is_open()) { 
    cout << "Open output file: " << gnup_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "Use -h options get more information!" << endl;
    return;
  } else {
    cout << "Generate gnuplot scripts: " << gnup_s << endl;
  }

  gnup << "reset session" << endl
       << endl
       << "set term wxt 2" << endl
       << "set font \"Times New Roman,10\"" << endl
       << "set multiplot" << endl
       << "set origin 0.5,0" << endl
       << "set size 0.5,1" << endl
       << "plot \"data/" << O_name << "_trajectories.txt\" u 1:3 lc -1 title 'up_g' with line,"\
       " \"data/" << O_name << "_trajectories.txt\" u 4:6 lc 7 title 'up_e' with line" << endl
       << "set grid" << endl
       << "set origin 0,0" << endl
       << "set size 0.5,1" << endl
       << "plot \"data/" << O_name << "_trajectories.txt\" u 2:3 lc -1 title 'side_g' with line,"\
       " \"data/" << O_name << "_trajectories.txt\" u 5:6 lc 2 title 'side_e' with line" << endl
       << "set grid" << endl
       << "unset multiplot" << endl
       << endl
       << "#plot x y z estimated compared with groundtruth" << endl
       << "set term wxt 3" << endl
       << "set font \"Times New Roman,10\"" << endl
       << "set multiplot" << endl
       << "set origin 0, 0.67" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_trajectories.txt\" u 7:1 lc -1 title 'x_g' with line,"\
       " \"data/" << O_name << "_trajectories.txt\" u 7:4 lc 7 title 'x_e' with line" << endl
       << "set grid" << endl
       << "set origin 0,0.33" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_trajectories.txt\" u 7:2 lc -1 title 'y_g' with line,"\
       " \"data/" << O_name << "_trajectories.txt\" u 7:5 lc 2 title 'y_e' with line" << endl
       << "set grid" << endl
       << "set origin 0,0" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_trajectories.txt\" u 7:3 lc -1 title 'z_g' with line,"\
       " \"data/" << O_name << "_trajectories.txt\" u 7:6 lc 3 title 'z_e' with line" << endl
       << "set grid" << endl
       << "unset multiplot" << endl
       << endl
       << "#plot x y z and sum error" << endl
       << "set term wxt 4" << endl
       << "set font \"Times New Roman,10\"" << endl
       << "#plot r p y error es-gt compare" << endl
       << "set multiplot" << endl
       << "set origin 0,0.5" << endl
       << "set size 1,0.5" << endl
       << "plot \"data/" << O_name << "_xyz_drift.txt\" u 9:1 lc 7 title 'drift_x' with line,"\
       " \"data/" << O_name << "_xyz_drift.txt\" u 9:2 lc 2 title 'drift_y' with line,"\
       " \"data/" << O_name << "_xyz_drift.txt\" u 9:3 lc 3 title 'drift_z' with line" << endl
       << "set grid" << endl
       << "set origin 0,0" << endl
       << "set size 1,0.5" << endl
       << "plot \"data/" << O_name << "_xyz_drift.txt\" u 9:4 lc -1 title 'drift_{sum}' with line"
       << endl
       << "set grid" << endl
       << "unset multiplot" << endl
       << endl
       << "#plot euler estimated compared to gt euler" << endl
       << "set term wxt 5" << endl
       << "set font \"Times New Roman,10\"" << endl
       << "set multiplot" << endl
       << "set origin 0, 0.67" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_euler.txt\" u 12:1 lc -1 title 'rx_g' with line,"\
       " \"data/" << O_name << "_euler.txt\" u 12:4 lc 7 title 'rx_e' with line" << endl
       << "set grid" << endl
       << "set origin 0,0.33" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_euler.txt\" u 12:2 lc -1 title 'ry_g' with line,"\
       " \"data/" << O_name << "_euler.txt\" u 12:5 lc 2 title 'ry_e' with line" << endl
       << "set grid" << endl
       << "set origin 0,0" << endl
       << "set size 1,0.4" << endl
       << "plot \"data/" << O_name << "_euler.txt\" u 12:3 lc -1 title 'rz_g' with line,"\
       " \"data/" << O_name << "_euler.txt\" u 12:6 lc 3 title 'rz_e' with line" << endl
       << "set grid" << endl
       << "unset multiplot" << endl
       << endl
       << "set term wxt 6" << endl
       << "set font \"Times New Roman,10\"" << endl
       << "#plot r p y error es-gt compare" << endl
       << "set multiplot" << endl
       << "set origin 0,0.5" << endl
       << "set size 1,0.5" << endl
       << "plot \"data/" << O_name << "_euler.txt\" u 12:7 lc 7 title 'error_{rx}' with line,"\
       " \"data/" << O_name << "_euler.txt\" u 12:8 lc 2 title 'error_{ry}' with line,"\
       " \"data/" << O_name << "_euler.txt\" u 12:9 lc 3 title 'error_{rz}' with line" << endl
       << "set grid" << endl
       << "set origin 0,0" << endl
       << "set size 1,0.5" << endl
       << "#plot angle axis error" << endl
       << "plot \"data/" << O_name << "_euler.txt\" u 12:11 lc -1 title 'Angle_g' with line,"\
       " \"data/" << O_name << "_euler.txt\" u 12:10 lc 9 title 'Angle_e' with line" << endl
       << "set grid" << endl
       << "unset multiplot" << endl
       << endl
       << "#plot trajectories" << endl
       << "reset session" << endl
       << "set term wxt 1" << endl
       << "set view equal xy" << endl
       << "set font \"Times New Roman,10\"" << endl
       << "splot \"data/" << O_name << "_trajectories.txt\" u 1:2:3 lc 3"\
          " title 'estimate' with line,"\
          "\"data/" << O_name << "_trajectories.txt\" u 4:5:6 lc -1"\
          " title 'groundtruth' with line" << endl
       << "set grid" << endl;

  gnup.close();

  string bsh_s = conf.output_dir+"/"+O_name+"_es2gt_plot.sh";
  ofstream bsh;
  bsh.open(bsh_s, ios::out|ios::trunc);
  if (!bsh.is_open()) {
    cout << "failed open output file: " << gnup_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "use -h options get more information!" << endl;
    return;
  } else {
    cout << "generate plot bash script: " << bsh_s << endl;
  }

  bsh << "#!/bin/bash" << endl
      << "gnuplot -e \"load '" << O_name << "_es2gt_gnuplot.sh'\" --persist" << endl
      << "#gnuplot" << O_name << "_es2gt_gnuplot.sh'\" --persist" << endl;

  bsh.close();
}
